#include "LdPcl.h"
#include <pcl/console/time.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>

typedef pcl::PointXYZINormal PointTypeFull;

bool IntensitySimilarity(const PointTypeFull &point_a, const PointTypeFull &point_b, float radiu)
{
    if (abs(point_a.intensity - point_b.intensity) < 30.0f)
        return true;
}

bool IntensityNormalSimilarity(const PointTypeFull &point_a, const PointTypeFull &point_b, float radiu)
{
    Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap(), point_b_normal = point_b.getNormalVector3fMap();
    if (std::abs(point_a.intensity - point_b.intensity) < 15.0f)
        // return true;
        if (std::abs(point_a_normal.dot(point_b_normal)) > std::cos(20.0f / 180.0f * static_cast<float>(M_PI)))
            return true;

    return false;
}

int main()
{
    LdsLidar &my_Lidar = LdsLidar::GetInstance();
    int initresult = my_Lidar.InitLdsLidar("auto", 1000);
    if (!initresult)
        std::cout << "Init success! " << std::endl;

    // Pcloud cloud2(new Cloud());
    // cloud2 = my_Lidar.GetPointCloud(1);
    // sleep(1);
    // while (cloud2 == nullptr || cloud2->size() < 200000)
    // {
    //     sleep(0.01);
    //     cloud2 = my_Lidar.GetPointCloud();
    // }
    // pcl::visualization::CloudViewer V_cloud2("Viewer cloud");
    // V_cloud2.showCloud(cloud2);
    // while (!V_cloud2.wasStopped())
    // {
    // }

    PcloudI cloudI(new CloudI());
    cloudI = my_Lidar.GetPointCloud_I(1);
    sleep(1);
    while (cloudI == nullptr || cloudI->size() < 200000)
    {
        sleep(0.01);
        cloudI = my_Lidar.GetPointCloud_I();
    }
    Pcloud cloud2(new Cloud());
    for (size_t i = 0; i < cloudI->points.size(); i++)
    {
        cloud2->points.push_back(Point(cloudI->points[i].x, cloudI->points[i].y, cloudI->points[i].z));
    }

    pcl::visualization::CloudViewer V_origin("Viewer Original cloud");
    V_origin.showCloud(cloudI);
    while (!V_origin.wasStopped())
    {
    }

    std::cout << "The cloud size is: " << cloudI->points.size() << std::endl;
    pcl::PointCloud<PointI>::Ptr cloud_in(new pcl::PointCloud<PointI>()), cloud_out(new pcl::PointCloud<PointI>());
    pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals(new pcl::PointCloud<PointTypeFull>());
    pcl::IndicesClustersPtr clusters(new pcl::IndicesClusters), small_clusters(new pcl::IndicesClusters), large_clusters(new pcl::IndicesClusters);
    pcl::search::KdTree<PointI>::Ptr search_tree(new pcl::search::KdTree<PointI>);
    pcl::console::TicToc tt;

    std::cerr << "降采样开始: \n", tt.tic();
    pcl::VoxelGrid<PointI> vg;
    vg.setInputCloud(cloudI);
    vg.setLeafSize(0.02f, 0.02f, 0.02f);
    vg.filter(*cloud_out);
    std::cout << "降采样完成!!!用时: " << tt.toc() << " ms" << cloud_out->size() << std::endl;

    pcl::copyPointCloud(*cloud_out, *cloud_with_normals);
    pcl::NormalEstimation<PointI, PointTypeFull> ne;
    ne.setInputCloud(cloud_out);
    ne.setSearchMethod(search_tree);
    // ne.setRadiusSearch(0.035);
    ne.setKSearch(30);
    ne.compute(*cloud_with_normals);
    std::cerr << "计算法线完成！！！用时: " << tt.toc() << " ms\n";

    std::cerr << "Segmenting to clusters...\n", tt.tic();
    pcl::ConditionalEuclideanClustering<PointTypeFull> cec(true);
    cec.setInputCloud(cloud_with_normals);
    cec.setConditionFunction(&IntensityNormalSimilarity);
    cec.setClusterTolerance(0.1);
    cec.setMinClusterSize(cloud_with_normals->size() / 2000);
    cec.setMaxClusterSize(cloud_with_normals->size() / 5);
    cec.segment(*clusters);
    cec.getRemovedClusters(small_clusters, large_clusters);
    std::cerr << ">> Done: " << tt.toc() << " ms\n";

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr All(new pcl::PointCloud<pcl::PointXYZRGB>());
    for (const auto &indice : (*clusters))
    {
        // Pcloud temp(new Cloud());
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGB>());
        int R = rand() % 255;
        int G = rand() % 255;
        int B = rand() % 255;
        for (const auto &idx : indice.indices)
        {
            float x = (*cloud_with_normals)[idx].x;
            float y = (*cloud_with_normals)[idx].y;
            float z = (*cloud_with_normals)[idx].z;
            pcl::PointXYZRGB tPoint;
            tPoint.x = x;
            tPoint.y = y;
            tPoint.z = z;
            tPoint.r = R;
            tPoint.g = G;
            tPoint.b = B;
            temp->push_back(tPoint);
            All->push_back(tPoint);
        }
        temp->width = temp->points.size();
        temp->height = 1;
        temp->is_dense = false;
        // pcl::visualization::CloudViewer cluster("Cluster_Viewer");
        // cluster.showCloud(temp);
        // while (!cluster.wasStopped())
        // {
        // }
    }
    All->height = 1;
    All->width = All->points.size();
    All->is_dense = false;
    std::cerr << "保存pcd...\n", tt.tic();
    std::string pcd_saved = "/home/milo/Documents/visual/2Champion/PCL/data/output.pcd";
    pcl::io::savePCDFile(pcd_saved, *All);
    std::cerr << "已保存！！！" << tt.toc() << std::endl;
    pcl::visualization::CloudViewer all("Viewer All");
    all.showCloud(All);
    while (!all.wasStopped())
    {
    }
    return 0;
}